#include <trap.h>
#include <nios2.h>
#include <defs.h>
#include <mmu.h>
#include <memlayout.h>
#include <clock.h>
#include <stdio.h>
#include <assert.h>
#include <vmm.h>
#include <unistd.h>
#include <syscall.h>
#include <error.h>
#include <sched.h>
#include <sync.h>
#include <libs_nios2.h>
#include <system.h>
#include <altera_avalon_timer_regs.h>

void inline
print_regs(struct pushregs *regs) {
    cprintf("  r15  0x%08x\n", regs->r15);
    cprintf("  r14  0x%08x\n", regs->r14);
    cprintf("  r13  0x%08x\n", regs->r13);
    cprintf("  r12  0x%08x\n", regs->r12);
    cprintf("  r11  0x%08x\n", regs->r11);
    cprintf("  r10  0x%08x\n", regs->r10);    
    cprintf("  r9  0x%08x\n", regs->r9);
    cprintf("  r8  0x%08x\n", regs->r8);
    cprintf("  r7  0x%08x\n", regs->r7);
    cprintf("  r6  0x%08x\n", regs->r6);
    cprintf("  r5  0x%08x\n", regs->r5);
    cprintf("  r4  0x%08x\n", regs->r4);
    cprintf("  r3  0x%08x\n", regs->r3);
    cprintf("  r2  0x%08x\n", regs->r2);
    cprintf("  r1  0x%08x\n", regs->r1);
}

void inline
print_trapframe(struct trapframe *tf) {
    cprintf("trapframe at %p\n", tf);
    cprintf("  ea  0x%08x\n", tf->ea);
    cprintf("  estatus  0x%08x\n", tf->estatus);
    print_regs(&tf->tf_regs);
    cprintf("  ra  0x%08x\n", tf->ra);
    cprintf("  sp  0x%08x\n", tf->sp);
}

bool
trap_in_kernel(struct trapframe *tf) {
    return !(tf->estatus & NIOS2_STATUS_U_MSK);
}

static void print(uintptr_t _la) {
        int i;
        for (i = 0; i < 10; ++i) {
            uintptr_t la = (_la) + i * 4;
            pte_t *ptep = get_pte(NIOS2_PGDIR, la, 0);
            if (*ptep & PTE_P) {
                uintptr_t pa_ppn = PPN(*ptep);
                uint32_t *addr = (uint32_t)(((pa_ppn << 12) | (la & 0xFFF)) + 0xC0000000);
                cprintf("trap : la=0x%x pa_ppn=0x%x addr=0x%x ", la, pa_ppn, addr);
                cprintf("value=0x%x\n", *addr);
            } else {
                cprintf("trap : la=0x%x !PTE_P!\n", la);
            }
            //cprintf("0x%x : ", (tf->ea)+i);
            //cprintf("0x%x\n", *((unsigned*)(tf->ea+i)));
        }

}

static inline int
irq_dispatch(struct trapframe *tf) {
    int irq_count = 0;
    if (tf->estatus & NIOS2_STATUS_PIE_MSK) {
        uint32_t ipending;
        NIOS2_READ_IPENDING(ipending);
        while (ipending) {
            uint32_t mask = 1;
            uint32_t i = 0;
            while (!(ipending & mask)) {
                mask <<= 1;
                i++;
            }
            ++irq_count;
            switch (i) {
            case TIMER_IRQ:
                IOWR_ALTERA_AVALON_TIMER_STATUS((TIMER_BASE + 0xC0000000), 0);
                ticks++;
                if (ticks % 1000 == 0)
                    cprintf("1000 ticks\n");
                break;
            default:
                panic("unknown irq %d\n", i);
                break;
            }
            NIOS2_READ_IPENDING(ipending);
        }
    }
    return irq_count;
}

static inline void
exception_dispatch(struct trapframe *tf) {
    uint32_t cause, badaddr, pteaddr;
    NIOS2_READ_EXCEPTION(cause);
    cause = (cause >> 2) & 0x1F;
    
    NIOS2_READ_BADADDR(badaddr);
    NIOS2_READ_PTEADDR(pteaddr);

    switch (cause) {
    case 0://NO TRAP
        break;
    case T_TRAP://syscall
        tf->ea += 4;
        syscall();
        break;
    case T_UNIMPLE://Unimplemented instruction
        panic("Unimplemented instruction! ea=0x%x\n", tf->ea);
        break;
    case T_ILLEGAL://Illegal instruction
        panic("Illegal instruction! cause=%d badaddr=0x%x\n", cause, badaddr);
        break;
    case T_MISALIGN_DATA://Misaligned data address
        print_trapframe(tf);
        panic("Misaligned data address! ea=0x%x badaddr=0x%x\n", tf->ea, badaddr);
        break;
    case T_MISALIGN_DEST://Misaligned destination address
        panic("Misaligned destination address! ea=0x%x\n", tf->ea);
        break;
    case T_DIVISION://Division error
        panic("Division error! ea=0x%x\n", tf->ea);
        break;
    case T_SUPERONLY_INSADDR://Supervisor-only instruction address
        panic("Supervisor-only instruction address! ea=0x%x\n", tf->ea);
        break;
    case T_SUPERONLY_INS://Supervisor-only instruction
        panic("Supervisor-only instruction! ea=0x%x\n", tf->ea);
        break;
    case T_SUPERONLY_DATA://Supervisor-only data address
        panic("Supervisor-only data address! badaddr=0x%x ea=0x%x\n", badaddr, tf->ea);
        break;
    case T_TLB_MISS://Fast TLB miss or Double TLB miss
        //cprintf("tlb_miss: tf->sp=0x%x\n", tf->sp);
        tlb_miss_handler(((pteaddr >> 2) & 0xFFFFF) << 12);
        break;
    case T_TLB_PERMISSION_EXE://TLB permission violation (execute)
        panic("TLB permission violation(execute)! pteaddr=0x%x  badaddr=0x%x\n", pteaddr, badaddr);
        break;
    case T_TLB_PERMISSION_READ://TLB permission violation (read)
        panic("TLB permission violation(read)! pteaddr=0x%x  badaddr=0x%x\n", pteaddr, badaddr);
        break;
    case T_TLB_PERMISSION_WRITE://TLB permission violation (write)
        panic("TLB permission violation(write)! pteaddr=0x%x  badaddr=0x%x\n", pteaddr, badaddr);
        break;
    default:
        panic("unknown exception! cause=%d. System is stopped.\n", cause);
        break;
    }
}

static void
trap_dispatch(struct trapframe *tf) {
    if (irq_dispatch(tf) == 0) {
        //if there is no irq, process exception.
        exception_dispatch(tf);
    }    
}

void
trap(struct trapframe *tf) {
    int status;
    NIOS2_READ_STATUS(status);
    NIOS2_WRITE_STATUS(status & (~NIOS2_STATUS_EH_MSK));
    // dispatch based on what type of trap occurred
    // used for previous projects
    if (current == NULL) {
        trap_dispatch(tf);
    }
    else {
        // keep a trapframe chain in stack
        struct trapframe *otf = current->tf;
        current->tf = tf;
	
        bool in_kernel = trap_in_kernel(tf);
	
        trap_dispatch(tf);
	
        current->tf = otf;
        if (!in_kernel) {
			if (current->flags & PF_EXITING) {
                do_exit(-E_KILLED);
            }
            if (current->need_resched) {
                schedule();
            }
        }
    }
}


